#include "axmapcontrol.h"
#include <stdio.h>
#include "AxRangeReading.h"

struct AxMapControl::pImpl
{
	AxMapControl* widget_;
	QMutex mutex_;
	int echo_size_;
	QColor clear_color_;
	AxRangeReading pointCloudOneFrame;
	std::vector<ReadingPoint> points;
	vector<QColor> plot_distance_colors_;
	vector<QColor> plot_intensity_colors_;

	vector3_t pose_;
	long timestamp;
	bool is_plot_data_updated_;
	bool is_updated_;

	pImpl(AxMapControl* widget):widget_(widget),
		is_plot_data_updated_(false),is_updated_(false),clear_color_(Qt::white),echo_size_(1)
	{
		plot_distance_colors_.push_back(QColor(0, 0, 128/255.0));
		plot_distance_colors_.push_back(QColor(0, 1.0, 1.0));
		plot_distance_colors_.push_back(QColor(30/255.0, 144/255.0, 255/255.0));
		plot_intensity_colors_.push_back(QColor(255/255.0, 69/255.0, 0.0));
		plot_intensity_colors_.push_back(QColor(1.0, 0.0, 1.0));
		plot_intensity_colors_.push_back(QColor(255/255.0, 215/255.0, 0/255.0));
	}
	void addPoints(ReadingPoints pts) 
	{
		pointCloudOneFrame.push_back(pts);
	}
	void clearPoints() 
	{
		pointCloudOneFrame.clear();
	}
};

AxMapControl::AxMapControl(QWidget *parent)
	: QGLViewer(parent),pimpl(new pImpl(this))
{
	ui.setupUi(this);
}

AxMapControl::~AxMapControl()
{

}

void draw_lines(const Points& points)
{
	glBegin(GL_LINES);
	size_t n = points.size();
	for (size_t i = 0; i < n; i += 2) {
		const vector_t& from = points[i];
		const vector_t& to = points[i + 1];
		glVertex2d(from.x, from.y);
		glVertex2d(to.x, to.y);
	}
	glEnd();
}


void draw_line_strip(const Points_group& points_group)
{
	for (Points_group::const_iterator it = points_group.begin();it != points_group.end(); ++it) 
	{
			glBegin(GL_LINE_STRIP);
			const Points& points = *it;
			size_t n = points.size();
			for (size_t i = 0; i < n; ++i) {
				const vector_t& p = points[i];
				glVertex2d(p.x, p.y);
			}
			glEnd();
	}
}


void draw_points(const Points& points)
{
	glBegin(GL_POINTS);
	for (Points::const_iterator it = points.begin();it != points.end(); ++it)
	{
			const vector_t& p = *it;
			glVertex2d(p.x, p.y);
	}
	glEnd();
}
void draw_3d_points(const ReadingPoints& points)
{
	glBegin(GL_POINTS);
	for (ReadingPoints::const_iterator it = points.begin();it != points.end(); ++it) 
	{
			const ReadingPoint& p = *it;
			glVertex3d(p.x, p.y,p.z);
	}
	glEnd();
}

void AxMapControl::set_plot_data(ReadingPoints pts,long timestamp)
{
	QMutexLocker locker(&pimpl->mutex_);

	pimpl->points.swap(pts);
	pimpl->addPoints(pts);
	pimpl->timestamp = timestamp;

	pimpl->is_plot_data_updated_ = true;
	pimpl->is_updated_ = true;
}

void AxMapControl::set_plot_data(ReadingPoints pts,vector3_t pose,long timestamp)
{
	QMutexLocker locker(&pimpl->mutex_);

	pimpl->points.swap(pts);
	pimpl->addPoints(pts);
	pimpl->timestamp = timestamp;
	pimpl->pose_ = pose;
	pimpl->is_plot_data_updated_ = true;
	pimpl->is_updated_ = true;
}

void AxMapControl::clear_plot_data()
{
	QMutexLocker locker(&pimpl->mutex_);
	pimpl->clearPoints();
	pimpl->is_plot_data_updated_ = true;
	pimpl->is_updated_ = true;

}

void AxMapControl::save()
{
	QMutexLocker locker(&pimpl->mutex_);
	SYSTEMTIME st; 
	GetLocalTime( &st );  
	char output_file[32];
	char output_3dfile[32];
	char output_pose[32];
	sprintf(output_file, "%4d-%2d-%2d-%2d-%2d-%2d.csv",st.wYear,st.wMonth,st.wDay,st.wHour,st.wMinute,st.wSecond);
	sprintf(output_3dfile,"%4d-%2d-%2d-%2d-%2d-%2d-3d.txt",st.wYear,st.wMonth,st.wDay,st.wHour,st.wMinute,st.wSecond);
	sprintf(output_pose,"%4d-%2d-%2d-%2d-%2d-%2d-3d.pose",st.wYear,st.wMonth,st.wDay,st.wHour,st.wMinute,st.wSecond);
	/*fd = fopen(output_file, "w");*/
	FILE* f3d =fopen(output_3dfile,"w");
	if (!f3d) 
	{
		perror("fopen");
		return;
	}
	//if (pimpl->pointCloudOneFrame.size()>0)
	//{
	//	for (vector<Point3ds>::const_iterator it = pimpl->pointCloudOneFrame.begin();it != pimpl->pointCloudOneFrame.end(); ++it) 
	//	{
	//		const Point3ds& pts = *it;
	//		if (pts.size()>0)
	//		{
	//			for (Point3ds::const_iterator it1 = pts.begin();it1 !=pts.end(); ++it1) 
	//			{
	//				const vector3_t& p = *it1;
	//				fprintf(f3d, "%.6f %.6f %.6f\n", p.x, p.y,p.z);
	//			}			
	//		}
	//	}
	//}
	if (f3d)
	{
		fclose(f3d);
	}
}

void AxMapControl::redraw(void)
{
	if (pimpl->is_updated_) {
		updateGL();
		pimpl->is_updated_ = false;
	}
}
void AxMapControl::draw()
{
		if (pimpl->points.size()>0)
		{
			QMutexLocker locker(&pimpl->mutex_);
			glColor3f(255/255.0, 69/255.0, 0.0);
			draw_3d_points(pimpl->points);
		}
		if (pimpl->pointCloudOneFrame.size()>0)
		{
			QMutexLocker locker(&pimpl->mutex_);
			glColor3f(255/255.0, 69/255.0, 0.0);
			for (AxRangeReading::const_iterator it = pimpl->pointCloudOneFrame.begin();it != pimpl->pointCloudOneFrame.end(); ++it) 
			{
				const ReadingPoints& p = *it;
				if (p.size()>0)
				{
					draw_3d_points(p);
				}
				
			}
		}
	/*const float nbSteps = 200.0;
	
	glBegin(GL_QUAD_STRIP);
	for (int i=0; i<nbSteps; ++i)
	{
	const float ratio = i/nbSteps;
	const float angle = 21.0*ratio;
	const float c = cos(angle);
	const float s = sin(angle);
	const float r1 = 1.0 - 0.8f*ratio;
	const float r2 = 0.8f - 0.8f*ratio;
	const float alt = ratio - 0.5f;
	const float nor = 0.5f;
	const float up = sqrt(1.0-nor*nor);
	glColor3f(1.0-ratio, 0.2f , ratio);
	glNormal3f(nor*c, up, nor*s);
	glVertex3f(r1*c, alt, r1*s);
	glVertex3f(r2*c, alt+0.05f, r2*s);
	}
	glEnd();*/
}

void AxMapControl::init()
{

}

QString AxMapControl::helpString() const
{
	return "";
}